
//Note to John: In this universe, all positive numbers equal all other positive numbers.
//So, if you try a conditional in which you check if an integer = anotherinteger, it will be true
//and a null pointer exception will eat the robot.

//
// Comms issues at Trenton Regional:
//  - driver station software has an issue that prevents linking to FMS
//      if multiple IP addresses are assigned to network adapter.  NOTE: spare parts
//      classmates have multiple IP addresses assigned - so they do not work without reconfig
//      NI issued patch on
//  - driver station software has issue linking with FMS if you've visited the
//      diagnostics tab(NI working on this)

// Data file analysis
//  - when elbow was stuck, NO COMMS ERRORS OCCURRED
//  - see if we can calc robot fps from data file info (should have enough)

// Ideas:
//  - write test program which spawns thread for CAN and times how long it
//      takes to setX and read params from all 8 Jags.  Log all exceptions, and
//      also log DS packets/teleopPeriodic timing
//  - update telemetry to be startable from Joystick button
//  - change arm control to use pre-programmed buttons & joystick finetuning (#2 below)
//  - consider if need to move CAN comms to separate thread (i.e. consider making
//      a CAN comm mgr static class with thread) - probably don't need to do this


// TODO:
//  1) autonomous mode selection & autonomous modes clean-up
//      a) use getDistanceTravelled()
//      b) turn more slowly (less than .05) when forward speed is > .05
//      c) make a console logger (so we save console output to a file as well) - DONE
//      d) add autonStep telem logging - DONE
//  2) pre-programmed arm positioning via gameStick buttons, with joystick
//      fine-tuning
//  3) Arm:
//      a) smooth out gotoPosition() control
//      b) add telem logging of Jag input voltage and faults - WAIT on this for now
//  4) DriveTrain:
//      a) smooth acceleration
//          - minimize/eliminate joystick/deadband
//          - accelerate more slowly in lower half (or lower .4) of joystick?
//      b) investigate gyro correction, particular on strafe
//          - try using captured performance data to see how control scheme
//          would work
//      c) add telem logging of Jag input voltage and faults - DONE
//  5) handle potential Jag brownouts in Arm & DriveTrain


package edu.archwood.frc2607.logomotion;

import com.sun.squawk.util.StringTokenizer;
import edu.archwood.frc2607.utility.ConsoleLogger;
import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStationLCD;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick.AxisType;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.can.CANTimeoutException;

public class LogomotionBot extends IterativeRobot
                           implements LogomotionConstants {
    int HOME = 450;
    int TopRack = 285;
    int MAX = 260;
    int HORIZONTAL = 362;
//edu.wpi.first.wpilibj.image.
    private ConsoleLogger cl = ConsoleLogger.getInstance();
    private LogomotionDriveTrain driveTrain = null;
    private LogomotionTelescopingArm theArm = null;
    private robovikingStick driveStick = null, gameStick = null;
    private LogomotionTelemetry dataLogger = null;
    private int loopCount = 0, autonStep = 0;
    private Compressor theCompressor = null;
    private DigitalInput poleSwitch;
    private Timer autonTimer = null;
    private DriverStationLCD lcd = DriverStationLCD.getInstance();
    private byte autonMode = Straightline_Mid;
    private Servo ShelfServo = null;
   // private Servo MinidServo = null;
    private long autonTime = 0;
    private long teleopTime = 0;
    private double autonDist = 0.0;
    private double spe[] = new double[101];
    private double turn[] = new double[101];
    private Camera TheCamera;
    private void initSpeedValues()
            
{
turn[0]=	0	;
turn[1]=	0	;
turn[2]=	0.35	;
turn[3]=	0.36575	;
turn[4]=	0.3840375	;
turn[5]=	0.403239375	;
turn[6]=	0.423401344	;
turn[7]=	0.444571411	;
turn[8]=	0.466799981	;
turn[9]=	0.490139981	;
turn[10]=	0.51464698	;
turn[11]=	0.540379329	;
turn[12]=	0.567398295	;
turn[13]=	0.59576821	;
turn[14]=	0.62555662	;
turn[15]=	0.656834451	;
turn[16]=	0.689676174	;
turn[17]=	0.724159982	;
turn[18]=	0.760367982	;
turn[19]=	0.798386381	;
turn[20]=	0.8383057	;
turn[21]=	0.880220985	;
turn[22]=	0.924232034	;
turn[23]=	0.970443636	;
turn[24]=	1.018965817	;
turn[25]=	1.069914108	;
turn[26]=	1.123409814	;
turn[27]=	1.179580304	;
turn[28]=	1.23855932	;
turn[29]=	1.300487286	;
turn[30]=	1.36551165	;
turn[31]=	1.433787232	;
turn[32]=	1.505476594	;
turn[33]=	1.580750424	;
turn[34]=	1.659787945	;
turn[35]=	1.742777342	;
turn[36]=	1.829916209	;
turn[37]=	1.92141202	;
turn[38]=	2.017482621	;
turn[39]=	2.118356752	;
turn[40]=	2.224274589	;
turn[41]=	2.335488319	;
turn[42]=	2.452262735	;
turn[43]=	2.574875871	;
turn[44]=	2.703619665	;
turn[45]=	2.838800648	;
turn[46]=	2.980740681	;
turn[47]=	3.129777715	;
turn[48]=	3.2862666	;
turn[49]=	3.45057993	;
turn[50]=	3.623108927	;
turn[51]=	3.804264373	;
turn[52]=	3.994477592	;
turn[53]=	4.194201472	;
turn[54]=	4.403911545	;
turn[55]=	4.624107122	;
turn[56]=	4.855312479	;
turn[57]=	5.098078103	;
turn[58]=	5.352982008	;
turn[59]=	5.620631108	;
turn[60]=	5.901662663	;
turn[61]=	6.196745797	;
turn[62]=	6.506583086	;
turn[63]=	6.831912241	;
turn[64]=	7.173507853	;
turn[65]=	7.532183245	;
turn[66]=	7.908792408	;
turn[67]=	8.304232028	;
turn[68]=	8.71944363	;
turn[69]=	9.155415811	;
turn[70]=	9.613186602	;
turn[71]=	10.09384593	;
turn[72]=	10.59853823	;
turn[73]=	11.12846514	;
turn[74]=	11.6848884	;
turn[75]=	12.26913282	;
turn[76]=	12.88258946	;
turn[77]=	13.52671893	;
turn[78]=	14.20305488	;
turn[79]=	14.91320762	;
turn[80]=	15.658868	;
turn[81]=	16.4418114	;
turn[82]=	17.26390197	;
turn[83]=	18.12709707	;
turn[84]=	19.03345192	;
turn[85]=	19.98512452	;
turn[86]=	20.98438075	;
turn[87]=	22.03359978	;
turn[88]=	23.13527977	;
turn[89]=	24.29204376	;
turn[90]=	25.50664595	;
turn[91]=	26.78197825	;
turn[92]=	28.12107716	;
turn[93]=	29.52713102	;
turn[94]=	31.00348757	;
turn[95]=	32.55366195	;
turn[96]=	34.18134504	;
turn[97]=	35.8904123	;
turn[98]=	37.68493291	;
turn[99]=	39.56917956	;
turn[100]=	41.54763853	;

for (int i = 0; i<=100;i++)
{
    turn[i]/=2;
}

spe[0]=	0	;
spe[1]=	0	;
spe[2]=	0.35	;
spe[3]=	0.36575	;
spe[4]=	0.3840375	;
spe[5]=	0.403239375	;
spe[6]=	0.423401344	;
spe[7]=	0.444571411	;
spe[8]=	0.466799981	;
spe[9]=	0.490139981	;
spe[10]=	0.51464698	;
spe[11]=	0.540379329	;
spe[12]=	0.567398295	;
spe[13]=	0.59576821	;
spe[14]=	0.62555662	;
spe[15]=	0.656834451	;
spe[16]=	0.689676174	;
spe[17]=	0.724159982	;
spe[18]=	0.760367982	;
spe[19]=	0.798386381	;
spe[20]=	0.8383057	;
spe[21]=	0.880220985	;
spe[22]=	0.924232034	;
spe[23]=	0.970443636	;
spe[24]=	1.018965817	;
spe[25]=	1.069914108	;
spe[26]=	1.123409814	;
spe[27]=	1.179580304	;
spe[28]=	1.23855932	;
spe[29]=	1.300487286	;
spe[30]=	1.36551165	;
spe[31]=	1.433787232	;
spe[32]=	1.505476594	;
spe[33]=	1.580750424	;
spe[34]=	1.659787945	;
spe[35]=	1.742777342	;
spe[36]=	1.829916209	;
spe[37]=	1.92141202	;
spe[38]=	2.017482621	;
spe[39]=	2.118356752	;
spe[40]=	2.224274589	;
spe[41]=	2.335488319	;
spe[42]=	2.452262735	;
spe[43]=	2.574875871	;
spe[44]=	2.703619665	;
spe[45]=	2.838800648	;
spe[46]=	2.980740681	;
spe[47]=	3.129777715	;
spe[48]=	3.2862666	;
spe[49]=	3.45057993	;
spe[50]=	3.623108927	;
spe[51]=	3.804264373	;
spe[52]=	3.994477592	;
spe[53]=	4.194201472	;
spe[54]=	4.403911545	;
spe[55]=	4.624107122	;
spe[56]=	4.855312479	;
spe[57]=	5.098078103	;
spe[58]=	5.352982008	;
spe[59]=	5.620631108	;
spe[60]=	5.901662663	;
spe[61]=	6.196745797	;
spe[62]=	6.506583086	;
spe[63]=	6.831912241	;
spe[64]=	7.173507853	;
spe[65]=	7.532183245	;
spe[66]=	7.908792408	;
spe[67]=	8.304232028	;
spe[68]=	8.71944363	;
spe[69]=	9.155415811	;
spe[70]=	9.613186602	;
spe[71]=	10.09384593	;
spe[72]=	10.59853823	;
spe[73]=	11.12846514	;
spe[74]=	11.6848884	;
spe[75]=	12.26913282	;
spe[76]=	12.88258946	;
spe[77]=	13.52671893	;
spe[78]=	14.20305488	;
spe[79]=	14.91320762	;
spe[80]=	15.658868	;
spe[81]=	16.4418114	;
spe[82]=	17.26390197	;
spe[83]=	18.12709707	;
spe[84]=	19.03345192	;
spe[85]=	19.98512452	;
spe[86]=	20.98438075	;
spe[87]=	22.03359978	;
spe[88]=	23.13527977	;
spe[89]=	24.29204376	;
spe[90]=	25.50664595	;
spe[91]=	26.78197825	;
spe[92]=	28.12107716	;
spe[93]=	29.52713102	;
spe[94]=	31.00348757	;
spe[95]=	32.55366195	;
spe[96]=	34.18134504	;
spe[97]=	35.8904123	;
spe[98]=	37.68493291	;
spe[99]=	39.56917956	;
spe[100]=	41.54763853	;
sprint = 100/spe[100];
}
    double sprint = 2.1D;
private Servo MinidServo;
    public void robotInit() {
        //assert (false):"JOHN!";
        // NOTE:  robotInit() won't exit if there are any ctor problems
        //  with the Jags (DriveTrain & Arm retry indefinitely)
        
        cl.log("Entering robotInit()");
        lcd.println(DriverStationLCD.Line.kMain6, 1,
                    "DriveTrain...       ");
        lcd.updateLCD();
        driveTrain = new LogomotionDriveTrain();
        driveStick = new robovikingStick(1);
        gameStick = new robovikingStick(2);
        ShelfServo = new Servo(DeploymentServoPWM);
        MinidServo = new Servo(ReleaseServoPWM);
        poleSwitch = new DigitalInput(8);
        //ShelfServo.set(0);
        lcd.println(DriverStationLCD.Line.kMain6, 1,
                    "Arm...              ");
        lcd.updateLCD();
        theArm = new LogomotionTelescopingArm();
        theCompressor = new Compressor(compressorSwitchDI, compressorRelay);
        theCompressor.start();

        dataLogger = new LogomotionTelemetry();
        dataLogger.addLoggedObject(this);
        dataLogger.addLoggedObject(driveTrain);
        dataLogger.addLoggedObject(theArm);
        autonTimer = new Timer();
                initSpeedValues();
        lcd.println(DriverStationLCD.Line.kMain6, 1,
                    "robotInit() DONE!   ");
        lcd.updateLCD();
        cl.log("Done robotInit()");
        TheCamera = new Camera();
    }

    public void autonomousInit() {
        cl.log("Entering autonomousInit()");
        loopCount = 0;
        autonStep = 0;
        MinidServo.setAngle(220);
        ShelfServo.setAngle(360);
        autonTimer.start();
        autonTimer.reset();
        dataLogger.setLoggingInterval(700);
        autonDist = 0.0;
        cl.log("Leaving autonomousInit()");
    }
int runner = 0, spit = 0;
int step = 0;
boolean done = false, hung = false;
    public void autonomousPeriodic() {
        long time = System.currentTimeMillis();
        autonDist = driveTrain.getDistanceTravelled();
        //autonMode = (byte)0;
       // autonMode=(byte)4000000;
      //  if (DriverStation.getInstance().getAlliance()==DriverStation.Alliance.kBlue) return;

switch (autonMode)
{
    case (byte)3:
        //punchSteve();
           break;
    case (byte)255:
        switch (step)
        {
            case 0:
                
        if (Math.abs(driveTrain.getGyroAngle())< 180||autonTimer.get() < 1)
        {
            driveTrain.mecanumDrive(0, 0,.05);
        }
        else
        {
            step+=1;
            autonTimer.reset();
        }
        break;
            case 1:
                if (theArm.gotoPosition(618) || autonTimer.get() > .5)
                {
                    step+=1;
                    autonTimer.reset();
                }
                break;
            case 2:
                theArm.rotateShoulder(0);
        
            theArm.useTheElbow(true);
        
                if (autonTimer.get() > 1)
                {
                    autonStep+=1;
                    autonTimer.reset();
            
                theArm.useTheElbow(false);
            
                }
                break;
                
        }
    
        break;
    case 2:
        case 1:
            /*if (3==8)
            {
                this.getWatchdog().kill();
                throw new NullPointerException();
                //CANCEL THE WATCHDOG!
            }*/
        //TODO: Add mode
         runner += 1;
         int travel = autonMode == 2?193:200;
         if ( runner > 0 && runner < 18)
         {
        theArm.gotoPosition(HORIZONTAL);
         }
         if (runner > 18 && runner < 20)
                  {
             theArm.useTheElbow(true);
                    }
         if ( runner > 20 && runner < 120)
         {
        theArm.gotoPosition(285);
         }
        if (runner > 120 && runner < 122)
        {
            theArm.rotateShoulder(0);
          //  theArm.telescopeArm(autonMode);
        }
        if (!done)
        {
            if (autonMode==2)
           driveTrain.followLine(.12, .07);
            else
              driveTrain.followFork(.12, .07,true);
           if (runner > 700 || autonDist >= travel)
           {
               done = true;
           }
        }
        else
        {
             if (!hung)
             {
            driveTrain.mecanumDrive(0,0,0);
            autonTimer.reset();
            try {
                theArm.suckOrSpitTube((byte) -1);
               hung = false;
               if (spit > 50 && spit < 200)
                       theArm.gotoPosition(317);
               spit += 1;
               if (spit > 200)
               {
                   hung = true;
                   theArm.rotateShoulder(0);
                   theArm.suckOrSpitTube((byte)0);
               }
            }
            catch (CANTimeoutException ex) {
                ex.printStackTrace();
            }
            }
             else
             {
                 theArm.rotateShoulder(0);
                // driveTrain.mecanumDrive(.1,0,0);
                     driveTrain.mecanumDrive(.1,0,0);
                 
             }
        }
        break;
        case Straightline_Low:
        //TODO: Add mode 880, 916
            runner += 1;
         if (runner < 20)
         {
        theArm.gotoPosition(880);
            }
        if (runner > 20)
        {
            theArm.rotateShoulder(0);
          //  theArm.telescopeArm(autonMode);
        }
        if (runner > 20 && runner < 40)
        {
            theArm.useTheElbow(true);
        }
        else
        {
            theArm.useTheElbow(false);
        }
        if (!done)
        {
           done = driveTrain.followLine(.12, .03);
           if (runner > 500 || autonDist >= 200)
           {
               done = true;
           }
        }
        else
        {
             if (!hung)
             {
            driveTrain.mecanumDrive(0,0,0);
            try {
                theArm.suckOrSpitTube((byte) -1);
               hung = theArm.gotoPosition(300);
            }
            catch (CANTimeoutException ex) {
                ex.printStackTrace();
            }
            }
             else
             {
                 driveTrain.mecanumDrive(.2,0,0);
             }
        }
        break;

//Fork Lefts
        case Forkleft_Mid:
        //TODO: Add mode
        break;
        case Forkleft_Top:
        //TODO: Add mode
        break;
        case Forkleft_Low:
        //TODO: Add mode
        break;

//Fork Rights
        case Forkright_Mid:
        //TODO: Add mode
        break;
        case Forkright_Top:
        //TODO: Add mode
        break;
        case Forkright_Low:
    default:
        //TODO: Add mode
        break;
        }

time -= System.currentTimeMillis();
time *=-1;
autonTime = time;
 
}
boolean droppedShelf = false;
    private void straightLineAndHang() {
        // drive 190in (185in?) following straight line, hang on top middle peg,
        // then reverse as far as possible
        
    }

    public void disabledInit() {
        System.out.println("Entering disabledInit()");
        autonTimer.stop();
        //theCompressor.stop();
        loopCount = 0;
        autonTime = 0;
        teleopTime = 0;
        dataLogger.setLoggingInterval(2000);
        dataLogger.enableLogging(true);
        System.out.println("Leaving disabledInit()");
        driveTrain.resetLineTracker();
        autonMode = 0;
    }
    int punches = 0;
public void punchSteve()
    {
        if (punches < 100) return;
        punches+=1;
        punchSteve();
    }
double armMove=0.0;
    public void disabledPeriodic() {
      //  System.out.println(Runtime.getRuntime().freeMemory());
        driveTrain.mecanumDrive(0.0,0.0,0.0);
        if (driveStick.getButtonToggle(1))
        {
            autonMode+=1;
            if (autonMode>3)
            {
                autonMode=1;
            }
        }
            String msg = "Cookies";
            switch (autonMode)
            {
                case 1:
                    msg = "Center          ";
                break;
                case 2:
                    msg = "Sides          ";
                break;
                case 3:
                    msg = "No Auton/Self destruct           ";
                break;
                case Straightline_Mid:
                    msg = "FollowStraight Mid NOTHING          ";
                break;
                case (byte)99999999:
                    msg = "FollowStraight Top NOTHING          ";
                break;
                case Straightline_Low:
                    msg = "FollowStraight Low NOTHING          ";
                break;
                case Forkleft_Mid:
                    msg = "FollowLeft Mid NOTHING          ";
                break;
                case Forkleft_Top:
                    msg = "FollowLeft Top NOTHING          ";
                break;
                case Forkleft_Low:
                    msg = "FollowLeft Low NOTHING          ";
                break;
                case Forkright_Mid:
                    msg = "FollowRight Mid NOTHING          ";
                break;
                case Forkright_Top:
                    msg = "FollowRight Top NOTHING          ";
                break;
                case Forkright_Low:
                    msg = "FollowRight Low NOTHING          ";
                break;
                
            }
            lcd.println(DriverStationLCD.Line.kUser2, 1, msg);
          //  lcd.updateLCD();
      //  lcd.println(DriverStationLCD.Line.kUser3, 1,theArm.getSwitches());
            lcd.updateLCD();
/*
            disp += 1;
            if (disp >= 25)
            {
            System.out.println(theArm.getSwitches());
            disp = 0;
            }
*/
    }
int disp = 0;
boolean avaluegriffintoldmetomake = false;
    public void teleopInit() {
        System.out.println("Entering teleopInit()");
        loopCount = 0;
        driveTrain.resetGyro();
        dataLogger.setLoggingInterval(700);
        System.out.println("Leaving teleopInit()");
        MinidServo.setAngle(220);
        ShelfServo.setAngle(360);
        
    }

    public void teleopPeriodic() {
      //  this.getWatchdog().
//        System.out.println(theArm.getSwitches());
long time = System.currentTimeMillis();
        if (gameStick.getRawButton(8))
        {
            ShelfServo.setAngle( ShelfServo.getAngle() - 180);
            droppedShelf = true;
        }
if (gameStick.getRawButton(9))
        {
    if (droppedShelf)
    {
           MinidServo.setAngle(130);
        }
        }
if (!gameStick.getRawButton(9))
{
        MinidServo.setAngle(220);
}
if (!gameStick.getRawButton(8))
{
        ShelfServo.setAngle(360);
}
//No reason
        /*if (gameStick.getButtonToggle(6)){
            if (theCompressor.enabled()) {
                cl.log("Stopping compressor");
                theCompressor.stop();
            }
            else {
                cl.log("Starting compressor");
                theCompressor.start();
            }
        }
   
/*
 * COMMENT BACK IN SOON!
 */

if (Math.abs(gameStick.getY())>0.02)
{
           armMove = -gameStick.getY();
}

          /*  if (armMove < 0)
            {
                armMove/=2;
            }*/
           
            if (theArm.getPot()<=MAX)
            {
                armMove=Math.min(armMove,0);
            }
/*if (armMove<0)
{
    armMove/=2;
}*/
        if (Math.abs(armMove)<.2)
            {
                armMove = 0;
            }
                theArm.rotateShoulder(armMove);
            
        
    if (gameStick.getRawButton(10))theArm.gotoPosition(HOME);
    if (gameStick.getRawButton(11))theArm.gotoPosition(TopRack);
    if (gameStick.getRawButton(7))theArm.gotoPosition(HOME);
    if (gameStick.getRawButton(6))theArm.gotoPosition(TopRack);
                //Kills Chris
    //if (gameStick.getRawButton(1)||gameStick.getRawButton(3))theArm.gotoPosition(HORIZONTAL);
   
          //  if theArm.getPot() > 30
          //{
                //TheCamera.ServoServo.set(gameStick.getRawAxis(3));
                //TheCamera.ServoServoServo.set(gameStick.getRawAxis(3));
            if (gameStick.getRawButton(4))
                theArm.useTheElbow(false);
            
        else if (gameStick.getRawButton(5))
                theArm.useTheElbow(true);
                // else theArm.setElbowSpeed(0.0);
            
           // else theArm.setElbowSpeed(0.0);
        //}
//}
        //else {
    //        theArm.gotoPosition(150, 72);
       // }



                
        //OLD DRIVE CODE!
       /* driveTrain.mecanumDrive(driveStick.getY(), -driveStick.getX(),
                                -driveStick.getRawAxis(4)/2);*/



//Enhanced drive code, should work
                double multiple = 1;
                //IGNORE THIS
                //if (driveStick.getRawButton(12))
                    multiple = sprint;
/*driveTrain.mecanumDrive(multiple*sign(driveStick.getY())*spe[(int)Math.floor(Math.abs(driveStick.getY())*100)]/100
    , -(sign(driveStick.getX())*spe[(int)Math.floor(Math.abs(driveStick.getX())*100)]/100)
    ,-sign(driveStick.getRawAxis(4))*turn[(int)Math.floor(Math.abs(driveStick.getRawAxis(4))*100)]/100);*/
                    driveTrain.mecanumDrive2(driveStick.getY(), -driveStick.getX(),-driveStick.getRawAxis(4));
if (driveStick.getRawButton(3))
{
    driveTrain.followLine(.12, .05);
}
     if (driveStick.getRawButton(4))
{
    driveTrain.followFork(.12, .05,true);
}
        if (gameStick.getRawButton(1)) {
            try {
                theArm.suckOrSpitTube((byte) -1);
            }
            catch (CANTimeoutException ex) {
                ex.printStackTrace();
            }
        }
        else
        {
           if (gameStick.getRawButton(3)||gameStick.getRawButton(2)) {
                try {
                    theArm.suckOrSpitTube((byte) 1);
                    //^ I wrote this
                }
                catch (CANTimeoutException ex) {
                    ex.printStackTrace();
                }
        }
           else
           {
                try {
                    theArm.suckOrSpitTube((byte) 0);
                }
                catch (CANTimeoutException ex) {
                    ex.printStackTrace();
                }
        }
           }
        

         lcd.println(DriverStationLCD.Line.kMain6, 1,
                                                theArm.getPotValues());

         lcd.println(DriverStationLCD.Line.kUser2, 1,
                                        driveTrain.getSwitches());
         lcd.println(DriverStationLCD.Line.kUser3, 1,"Distance:"+driveTrain.getDistanceTravelled());
         lcd.println(DriverStationLCD.Line.kUser4, 1,"POT:"+theArm.getPotValues());
         lcd.println(DriverStationLCD.Line.kUser6, 1,
                                        poleSwitch.get()?"THE MINID IS IN POSITION!":"~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~");
//        lcd.println(DriverStationLCD.Line.kUser3, 1,theArm.getSwitches());
         String msg = "Gyro: " + String.valueOf(driveTrain.getGyroAngle()).substring(0,3);
         msg += " Temp: " + String.valueOf(driveTrain.getCurTemp()).substring(0,3);
         //I do know shun cakes
         lcd.println(DriverStationLCD.Line.kUser5, 1,
                                        msg);
         lcd.updateLCD();

/*
         StringTokenizer st = driveTrain.motorSPs();
         DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser2, 1,
                                        st.nextToken());
         DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser3, 1,
                                        st.nextToken());
*/
/*
         StringTokenizer st = driveTrain.motorPVs();
         DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser4, 1,
                                        st.nextToken());
         DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser5, 1,
                                        st.nextToken());
 *
 */
time -= System.currentTimeMillis();
time *=-1;
teleopTime = time;
    }

    public String getTelemetryHeader() {
        return "DS Packet,Mode,Battery,AutonStep,AutonTimer,AutonDist,-DriveX,DriveY,-DriveZ,Drive Btns,Game Btns,AutonTime,TeleopTime";
    }
public int sign(double A)
    {
    int ret = 0;
    try
    {//COMMENTS!
        if (A!=0)
        ret = (int)(A/Math.abs(A));
    }
    catch (Exception e)
    {
        
    }
    return ret;
    }
    public String getTelemetryData() {
        String msg = String.valueOf(m_ds.getPacketNumber()) + ",";    // DS Packet
        msg += (m_ds.isAutonomous() ? "Auto" : "Teleop");
        msg += (m_ds.isDisabled() ? "Disabled" : "Enabled") + ",";    // Mode
        msg += Double.toString(m_ds.getBatteryVoltage()) + ",";     // Voltage
        msg += Integer.toString(autonStep) + ",";                   // autonStep
        msg += Double.toString(autonTimer.get()) + ",";             // autonTimer
        msg += Double.toString(autonDist) + ",";                    // autonDistance
        msg += Double.toString(-driveStick.getX()) + ",";             // DriveX
        msg += Double.toString(driveStick.getY()) + ",";             // DriveY
        msg += Double.toString(-driveStick.getRawAxis(4)) + ",";      // DriveZ
        msg += Integer.toBinaryString(m_ds.getStickButtons(1)) + ","; // Drive Btns
        msg += Integer.toBinaryString(m_ds.getStickButtons(2)) + ",";       // Game Btns
        msg += Long.toString(autonTime) + ","; //AutonTime
        msg += Long.toString(teleopTime);       // TeleopTime
        return msg;
    }
}

//===========================================================================
/*
    public void autonomousPeriodic() {
        switch (autonStep) {
            case 0:
                if (driveTrain.driveToZone(-.15) || autonTimer.get() >= 5) {
                    ++autonStep;
                    autonTimer.reset();
                }
                break;
            case 1:
                driveTrain.mecanumDrive(0.0,0.0,0.0);
                if (theArm.gotoPosition(590,550)) {
                    ++autonStep;
                    autonTimer.reset();
                }
                break;
            case 2:
                theArm.setElbowSpeed(0.0);
                theArm.rotateShoulder(0.0);
                driveTrain.mecanumDrive(-.02,0.0,0.0);
                if (autonTimer.get() >= 1.5) {
                    ++autonStep;
                    autonTimer.reset();
                }
                break;
            case 3:
                if (theArm.gotoPosition(590,580)) {
                    ++autonStep;
                    autonTimer.reset();
                }
                break;
            case 4:
                theArm.openClaw();
                if (autonTimer.get() >= .7) {
                    ++autonStep;
                    autonTimer.reset();
                }
                break;
            case 5:
                driveTrain.mecanumDrive(.2,0.0,0.0);
                theArm.gotoPosition(135,20);
                if (autonTimer.get() >= 3) {
                    ++autonStep;
                    autonTimer.reset();
                }
                break;
            case 6:
                driveTrain.mecanumDrive(0.0,0.0,0.0);
                theArm.setElbowSpeed(0.0);
                theArm.rotateShoulder(0.0);
                break;
        }
    }
*/
 /*
        public void autonomousPeriodic() {

        // assume we start on the straight line.  Follow it and score on
        // middle peg
        boolean endOfLine;
        if (writeMsg) {
            System.out.println("autonStep: " + autonStep);
            writeMsg = false;
        }
        switch (autonStep) {
            //follow line for 3 seconds
            case 0:
                driveTrain.followLine(.09, .05);
                if (autonTimer.get() >= 3) { autonTimer.reset(); ++autonStep; writeMsg = true;}
                break;
           // continue following line but also move arm into position
            case 1:
                driveTrain.followLine(.09, .05);
                //160, 80
                theArm.gotoPosition(160,80);
                if (autonTimer.get() >= 3) {
                    autonTimer.reset();
                    ++autonStep;
                    writeMsg = true;
                }
                break;
           // stop the arm motors (just in case it's bouncing)
            case 2:
                theArm.setElbowSpeed(0.0);
                theArm.rotateShoulder(0.0);
                driveTrain.followLine(.08, .05);
                if (autonTimer.get() >= 2) {
                    autonTimer.reset();
                    ++autonStep;
                    writeMsg = true;
                }
                break;
            case 3:
                driveTrain.mecanumDrive(0.0, 0.0, 0.0);
                if (theArm.gotoPosition(160, 182) || autonTimer.get() >= 2) {
                    autonTimer.reset();
                    ++autonStep;
                    writeMsg = true;
                }
                break;
            case 4:
                theArm.setElbowSpeed(0.0);
                theArm.rotateShoulder(0.0);
                theArm.openClaw();
                if (autonTimer.get() >= 1) {
                    autonTimer.reset();
                    ++autonStep;
                    writeMsg = true;
                }
                break;
            case 5:
                if(theArm.gotoPosition(140, 180) || autonTimer.get() >= 2) {
                    autonTimer.reset();
                    ++autonStep;
                    writeMsg = true;
                }
                break;
            case 6:
                driveTrain.mecanumDrive(0.2, 0.0, 0.0);
                if (theArm.gotoPosition (135, 20) || autonTimer.get() >= 2) {
                    autonTimer.reset();
                    ++autonStep;
                    writeMsg = true;
                }
                break;
            case 7:
               driveTrain.mecanumDrive(0.0, 0.0, 0.0);
               theArm.setElbowSpeed(0.0);
               theArm.rotateShoulder(0.0);
               break;
        }
    }
*/

/*
    case Justdrive_Top:
    default:
        autonDist = driveTrain.getDistanceTravelled();
        switch (autonStep) {
            case 0:
                if (driveTrain.driveToZone(-.2) || autonTimer.get() >= 5.5) {
                    ++autonStep;
                    autonTimer.reset();
                }
                break;
            case 1:
                driveTrain.mecanumDrive(0.0,0.0,0.0);
                if (theArm.gotoPosition(590) || autonTimer.get() >= 2.5) {
                    ++autonStep;
                    autonTimer.reset();
                }
                break;
            case 2:
                //theArm.setElbowSpeed(0.0);
                theArm.rotateShoulder(0.0);
                driveTrain.mecanumDrive(-.02,0.0,0.0);
                if (autonTimer.get() >= 2) {
                    ++autonStep;
                    autonTimer.reset();
                }
                break;
            case 3:
                if (theArm.gotoPosition(590) || autonTimer.get() >= 1.5) {
                    ++autonStep;
                    autonTimer.reset();
                }
                break;
            case 4:
        try {
            theArm.suckOrSpitTube((byte) -1);
        }
        catch (CANTimeoutException ex) {
            ex.printStackTrace();
        }
                if (autonTimer.get() >= .7) {
                    ++autonStep;
                    autonTimer.reset();
                }
                break;
            case 5:
                driveTrain.mecanumDrive(.2,0.0,0.0);
                theArm.gotoPosition(135);
                if (autonTimer.get() >= 3) {
                    ++autonStep;
                    autonTimer.reset();
                }
                break;
            case 6:
                driveTrain.mecanumDrive(0.0,0.0,0.0);
               // theArm.setElbowSpeed(0.0);
                theArm.rotateShoulder(0.0);
                break;
        }
        break;
       case Justdrive_Mid:
        //TODO: Add mode
        break;
        case Justdrive_Low:
        //TODO: Add mode
        break;

//Straight Lines
    case Straightline_Top:
        //TODO: Add mode
                switch (autonStep) {
            case 0:
                autonDist = driveTrain.getDistanceTravelled();
                if (driveTrain.followLine(.12,.03)) {
                    driveTrain.mecanumDrive(0.0,0.0,0.0);
                    autonStep = 1;
                    autonTimer.reset();
                } else {
                    if (autonDist != Double.NaN && autonDist >= 160.0) {
                        autonStep = 2;
                        autonTimer.reset();
                    }
                }
                break;
            case 1:
                driveTrain.mecanumDrive(.08, 0, 0);
                if (autonTimer.get() >= 2) {
                    ++autonStep;
                    autonTimer.reset();
                }
                break;
            case 2:
                driveTrain.mecanumDrive(0.0,0.0,0.0);
                if (theArm.gotoPosition(590) || autonTimer.get() >= 1.5) {
                    ++autonStep;
                    autonTimer.reset();
                }
                break;
            case 3:
             //   theArm.setElbowSpeed(0.0);
                theArm.rotateShoulder(0.0);
//                driveTrain.mecanumDrive(-.02,0.0,0.0);
//                if (autonTimer.get() >= .5) {
                    ++autonStep;
                    autonTimer.reset();
//                }
                break;
            case 4:
                if (theArm.gotoPosition(590) || autonTimer.get() >= .5) {
                    ++autonStep;
                    autonTimer.reset();
                }
                break;
            case 5:
        try {
            theArm.suckOrSpitTube((byte) -1);
        }
        catch (CANTimeoutException ex) {
            ex.printStackTrace();
        }
                if (autonTimer.get() >= .7) {
                    ++autonStep;
                    autonTimer.reset();
                }
                break;
            case 6:
                driveTrain.mecanumDrive(.2,0.0,0.0);
                theArm.gotoPosition(135);
                if (autonTimer.get() >= 3) {
                    ++autonStep;
                    autonTimer.reset();
                }
                break;
            case 7:
                driveTrain.mecanumDrive(0.0,0.0,0.0);
              //  theArm.setElbowSpeed(0.0);
                theArm.rotateShoulder(0.0);
                break;
        }
        break;
 *
 */
